/*
  Morse.cpp - Library for flashing Morse code.
  Created by David A. Mellis, November 2, 2007.
  Released into the public domain.
*/

#include "Arduino.h"
#include "MouseKA8.h"
MouseKA8::MouseKA8(int pinSDA,int pinSCL)
{
	_pinsda = pinSDA;
	_pinscl = pinSCL;
  pinMode(_pinscl, OUTPUT);
  digitalWrite(_pinscl, HIGH);
  pinMode(_pinsda, OUTPUT);
  digitalWrite(_pinsda, HIGH);

}
void MouseKA8:: init_reg()
{
  uint8_t resolution = RESOLUTION;
  uint8_t sleep_mode = SLEEP_MODE;
  uint8_t tmp = 0;

  uint16_t product_id = 0;
  uint8_t product_id_tmp = read_reg(reg_Product_ID_1);
  Serial.print("Product ID 1 register value (default 0x30) : 0x");
  Serial.println(product_id_tmp, HEX);
  product_id = product_id_tmp << 4;
  delay(DELAY);

  product_id_tmp = read_reg(reg_Product_ID_2);
  Serial.print("Product ID 2 register value (default 0x5X) : 0x");
  Serial.println(product_id_tmp, HEX);
  product_id += product_id_tmp >> 4;
  delay(DELAY);

  if (product_id)
  {
    Serial.println(">> Communication with KA8 sensor OK");
    Serial.print("Product number : 0x");
    Serial.println(product_id, HEX);
  }
  else
  {
    Serial.println("Bad communication with the device");
  }
  delay(DELAY);
  Serial.println();


  tmp = read_reg(reg_Operation_Mode);
  Serial.print("Operation Mode register value (default 0xB8) : 0x");
  Serial.println(tmp, HEX);
  delay(DELAY);

  while (tmp != sleep_mode) {
    Serial.println();
    // Change the resolution Value (default 0x04, max 0x07, min 0x00)
    Serial.println("Changing the operation mode to desactivate sleep");
    write_reg(reg_Operation_Mode, sleep_mode);
    delay(DELAY);
    tmp = read_reg(reg_Operation_Mode);
    Serial.print("New operation mode value : ");
    Serial.println(tmp, HEX);
    delay(DELAY);
  }

  tmp = read_reg(reg_Configuration);
  Serial.print("Configuration register value (default 0x04) : 0x");
  Serial.println(tmp, HEX);
  delay(DELAY);

  while (tmp != resolution) {
    Serial.println();
    // Change the resolution Value (default 0x04, max 0x07, min 0x00)
    Serial.println("Changing the configuration value for resolution");
    write_reg(reg_Configuration, resolution);
    delay(DELAY);
    tmp = read_reg(reg_Configuration);
    Serial.print("New configuration value : ");
    Serial.println(tmp, HEX);
    delay(DELAY);
  }

  tmp = read_reg(reg_Image_Quality);
  Serial.print("Image quality register value : 0x");
  Serial.println(tmp, HEX);
  delay(DELAY);

  tmp = read_reg(reg_Operation_State);
  Serial.print("Operation state register value : 0x");
  Serial.println(tmp, HEX);
  delay(DELAY);

  tmp = read_reg(reg_Write_Protect);
  Serial.print("Write protect register value (default 0x00) : 0x");
  Serial.println(tmp, HEX);
  delay(DELAY);
}
uint8_t MouseKA8::read_reg(uint8_t reg)
{
	uint8_t data = 0;
  pinMode(_pinsda, OUTPUT);
  digitalWrite(_pinsda, HIGH);
  delayMicroseconds(1);

  for (int i = 0; i < 8; i++)
  {
    digitalWrite(_pinscl, LOW);

    if (reg & (0x01 << (7 - i) ) )
    {
      digitalWrite(_pinsda, HIGH);
    }
    else
    {
      digitalWrite(_pinsda, LOW);
    }
    digitalWrite(_pinscl, HIGH);
  }

  delayMicroseconds(2);
  pinMode(_pinsda, INPUT);
  delayMicroseconds(1);

  for (int i = 0; i < 8; i++)
  {
    digitalWrite(_pinscl, LOW);

    if (digitalRead(_pinsda))
    {
      data |= (0x01 << (7 - i));
    }
    digitalWrite(_pinscl, HIGH);
  }
  pinMode(_pinsda, OUTPUT);
  digitalWrite(_pinsda, LOW);

  // Latch the data for synchronization
  digitalWrite(_pinscl, LOW);
  delayMicroseconds(2);
  digitalWrite(_pinscl, HIGH);
  delay(2);

  return data;
}
void MouseKA8::write_reg(uint8_t reg, uint8_t data)
{
	reg |= (0x01 << 7);

  pinMode(_pinsda, OUTPUT);
  digitalWrite(_pinsda, HIGH);
  delayMicroseconds(1);

  for (int i = 0; i < 8; i++)
  {
    digitalWrite(_pinscl, LOW);

    if (reg & (0x01 << (7 - i) ) )
    {
      digitalWrite(_pinsda, HIGH);
    }
    else
    {
      digitalWrite(_pinsda, LOW);
    }
    digitalWrite(_pinscl, HIGH);
  }

  delayMicroseconds(2);
  delayMicroseconds(1);

  for (int i = 0; i < 8; i++)
  {
    digitalWrite(_pinscl, LOW);

    if (data & (0x01 << (7 - i) ) )
    {
      digitalWrite(_pinsda, HIGH);
    }
    else
    {
      digitalWrite(_pinsda, LOW);
    }
    digitalWrite(_pinscl, HIGH);
  }

  digitalWrite(_pinsda, LOW);

  // Latch the data for synchronization
  digitalWrite(_pinscl, LOW);
  delayMicroseconds(2);
  digitalWrite(_pinscl, HIGH);
  delay(2);
}
int MouseKA8::convTwosComp(int b) { //Convert from 2's complement
  if (b & 0x80) {
    b = -1 * ((b ^ 0xff) + 1);
  }
  return b;
}
short MouseKA8::position()
{
	uint8_t rms = read_reg(reg_Motion_Status);

  if (0x80 < rms && rms < 0x88) {
    uint8_t position_X = read_reg(reg_Delta_X);
    uint8_t position_Y = read_reg(reg_Delta_Y);

    if (convTwosComp(position_X) < -NOISE_FILTER || NOISE_FILTER < convTwosComp(position_X) || convTwosComp(position_Y) < -NOISE_FILTER || NOISE_FILTER < convTwosComp(position_Y)) {
      if (convTwosComp(position_X) == -88 &&  convTwosComp(position_Y) == 80) {
        Serial.println("Strange value X=-88 & Y=80");
      }
      else {
        //Serial.print(rms, BIN);
        Serial.print("  ---  X : ");
        Serial.print(convTwosComp(position_X));
        Serial.print("  --- Y : ");
        Serial.println(convTwosComp(position_Y));
        return (position_X<<8|position_Y);
      }
    }
  }
  else if (rms > 0xA0) {
    // Serial.print("Error reserved Pin : ");
    // Serial.println(rms, BIN);
  }
  else if (rms > 0x08) {
    // Serial.print("Error : ");
    // Serial.println(rms, BIN);
  }
}